#include <iostream>
#include <string>
#include <thread>
#include <chrono>
#include <unistd.h> // For close()
#include <sys/socket.h> // For socket(), connect()
#include <netinet/in.h> // For sockaddr_in
#include <arpa/inet.h> // For inet_pton()
#include <netdb.h> // For getaddrinfo(), freeaddrinfo()
#include <cstring> // For memset()
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <signal.h> // For signal handling
#include <atomic> // For atomic flag
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_datatypes.h>
#include <nlohmann/json.hpp>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>

using json = nlohmann::json;




const int SetPointGoal  = 1000;
const int SetCmdVel     = 1001;

const int HeartBeat     = 5000;
const int BindRobotID   = 5001;
const int Backpacking   = 5002;
const int GoalResult    = 5003;

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

struct MsgRecvData{
    int cmd;
    std::string data;
};

struct Point
{
    double x;
    double y;
    double yaw;
};

struct MsgSendData{
    int cmd;
    std::string data;
};

struct RobotInfo{
    int robotId ;
};

class TcpClient {
public:
    TcpClient(ros::NodeHandle& nh,const std::string& server_address, const std::string& server_port)
        :nh_(nh), server_address_(server_address), server_port_(server_port), sockfd_(-1), running_(true), shutdown_flag_(false),
        move_base_clent("move_base",true) {
             
             laser_sub = nh_.subscribe("/scan",10,&TcpClient::LaserCallBack,this);
             vel_cmd_pub = nh_.advertise<geometry_msgs::Twist>("/cmd_vel",10);
             init_pos_pub = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose",10);

        }

    ~TcpClient() {
        stop();
        if (sockfd_ != -1) {
            close(sockfd_);
        }
    }

    bool connectToServer() {
        struct addrinfo hints, *server_info, *p;
        int status;
        memset(&hints, 0, sizeof(hints));
        hints.ai_family = AF_UNSPEC;     // IPv4 or IPv6
        hints.ai_socktype = SOCK_STREAM; // TCP socket

        status = getaddrinfo(server_address_.c_str(), server_port_.c_str(), &hints, &server_info);
        if (status != 0) {
            std::cerr << "getaddrinfo error: " << gai_strerror(status) << std::endl;
            return false;
        }

        for (p = server_info; p != NULL; p = p->ai_next) {
            sockfd_ = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
            if (sockfd_ == -1) {
                continue;
            }
            if (connect(sockfd_, p->ai_addr, p->ai_addrlen) != -1) {
                break; // 成功连接
            }
            close(sockfd_);
            sockfd_ = -1;
        }

        if (sockfd_ == -1) {
            std::cerr << "Could not connect to server." << std::endl;
            return false;
        }
        sendRobotInfoToPss();
        ROS_INFO("connected TCPserver success");
        return true;
    }

    void startReceiving() {
        receive_thread_ = std::thread(&TcpClient::receiveMessages, this);
    }

    void joinReceiving() {
        if (receive_thread_.joinable()) {
            receive_thread_.join();
        }
        if (waitResult_thread_.joinable()){
            waitResult_thread_.join();
        }
    }

    void stop() {
        running_ = false;
        shutdown_flag_ = true;
        if (sockfd_ != -1) {
            shutdown(sockfd_, SHUT_RDWR); // 关闭读写
        }
    }

    bool get_shutdown_flag_()const{
        return shutdown_flag_;
    }

    void sendRobotInfoToPss(){
        json rb ={
           {"robotId",1}
        };
        json recvData = {
            {"cmd",BindRobotID},
            {"data",rb.dump()}
        };
         sendData(recvData);
    }

    void sendBackpacking(int cmd){
        json rb ={
            {"backpackCmd",cmd}
        };
        json recvData ={
            {"cmd" ,Backpacking},
            {"data",rb.dump()}
            
        };
        sendData(recvData);
    }

    void sendGoalWaitResult(bool result){
        json rb ={
            {"result",result}
        }; 
        json recvData = {
            {"cmd",GoalResult},
            {"data",rb.dump()}
        };
        sendData(recvData);   
    }

    bool sendData(const json& data){
        if (sockfd_ == -1) {
        std::cerr << "Socket not connected." << std::endl;
        return false;
        }

        std::string json_str=data.dump();

        ssize_t bytes_sent = send(sockfd_, json_str.c_str(), json_str.size(), 0);
        if (bytes_sent == -1) {
            std::cerr << "Error sending data: " << strerror(errno) << std::endl;
            return false;
        }

        std::cout << "Sent " << bytes_sent << " bytes of data: " << json_str << std::endl;
        return true;
    }

    void from_Point_json(const json& j ,Point& p){
        j.at("x").get_to(p.x);
        j.at("y").get_to(p.y);
        j.at("yaw").get_to(p.yaw);
    }

    void from_MsgData_json(const json& j,MsgRecvData& msg){
        j.at("cmd").get_to(msg.cmd);
        j.at("data").get_to(msg.data);
    }

    void setRobotInitialpose(double x,double y,double yaw){
        geometry_msgs::PoseWithCovarianceStamped pose;

        pose.header.frame_id="map";
        pose.header.stamp=ros::Time::now();

        pose.pose.pose.position.x=x;
        pose.pose.pose.position.y=y;

        pose.pose.pose.orientation.z=yaw;

        init_pos_pub.publish(pose);

        ros::Duration(1.0).sleep();

    }

    //
    void sendGoal(double x,double y,double yaw){
        move_base_msgs::MoveBaseGoal goal;
        //

        ROS_INFO("x: %.6f, y: %.6f, yaw: %.6f", x, y, yaw);

        // if(!move_base_clent.isServerConnected()){
        //     sendGoalWaitResult(false);
        //     return;
        // }

        goal.target_pose.header.frame_id="map";
        goal.target_pose.header.stamp=ros::Time::now();
        goal.target_pose.pose.position.x= x;
        goal.target_pose.pose.position.y= y;

        tf::Quaternion q =tf::createQuaternionFromYaw(yaw);
        goal.target_pose.pose.orientation.x=q.x();
        goal.target_pose.pose.orientation.y=q.y();
        goal.target_pose.pose.orientation.z=q.z();
        goal.target_pose.pose.orientation.w=q.w();

        ROS_INFO("x: %.6f, y: %.6f, z: %.6f ,w: %.6f", q.x(), q.y(), q.z(),q.w());

        ROS_INFO("Sending goal");

        move_base_clent.sendGoal(goal);

        startWaitResutl();
    }

    void waitResult(){
        move_base_clent.waitForResult();

        if (move_base_clent.getState()==actionlib::SimpleClientGoalState::SUCCEEDED){
            ROS_INFO("Goal reached");
            sendGoalWaitResult(true);
        }else{
            ROS_INFO("the base failed to reach the goal for some reason");
            sendGoalWaitResult(false);
        }
    }

    void startWaitResutl(){
        waitResult_thread_ = std::thread(&TcpClient::waitResult, this);
    }

    void joinswaitResult() {
        if (waitResult_thread_.joinable()){
            waitResult_thread_.join();
        }
    }


    void sendCmd_vel(double x,double y,double yaw){
        ROS_INFO("x: %.2f, y: %.2f, yaw: %.2f", x, y,yaw);

        geometry_msgs::Twist vel_cmd;

        vel_cmd.linear.x=x;
        
        vel_cmd.angular.z=yaw;

        vel_cmd_pub.publish(vel_cmd);
    }

    void LaserCallBack(const sensor_msgs::LaserScan msg){
        
    }

private:
    std::string server_address_;
    std::string server_port_;

    int sockfd_;
    std::thread receive_thread_;
    std::thread waitResult_thread_;
    bool running_;
    std::atomic<bool> shutdown_flag_;
    MoveBaseClient move_base_clent;
    ros::NodeHandle nh_;

    ros::Publisher vel_cmd_pub;
    ros::Publisher init_pos_pub;
    ros::Subscriber laser_sub;
    

    int dealwithReceivedData(const std::string& data) {

        json msg_Json = json::parse(data);
        MsgRecvData msgData ;
        
        from_MsgData_json(msg_Json,msgData);
        
        json data_json = json::parse(msgData.data);

        switch (msgData.cmd)
        {
        case SetPointGoal:
            Point goalpPoint;
            from_Point_json(data_json,goalpPoint);
            sendGoal(goalpPoint.x,goalpPoint.y,goalpPoint.yaw);
            break;
        case SetCmdVel:
            Point cmdPoint;
            from_Point_json(data_json,cmdPoint);
            sendCmd_vel(cmdPoint.x,cmdPoint.y,cmdPoint.yaw);
        default:
            break;
        }
        return msgData.cmd;
     
    }

    void receiveMessages() {
        char buffer[1024];
        while (running_ && !ros::isShuttingDown() && !shutdown_flag_) {
            memset(buffer, 0, 1024);
            int bytes_received = recv(sockfd_, buffer, 1024, 0);
            if (bytes_received <= 0) {
                if (bytes_received == 0) {
                    std::cerr << "Server closed connection." << std::endl;
                } else {
                    std::cerr << "Error receiving data" << std::endl;
                }
                break;
            }
            std::string received(buffer, bytes_received);
            int cmd = dealwithReceivedData(received);

            sendBackpacking(cmd);

            ROS_INFO_STREAM("Received Data: " << received);

            // 暂停一段时间
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }
};


// 信号处理函数
void signalHandler(int signum) {
    ROS_INFO("Signal %d caught, shutting down.", signum);
    ros::shutdown();
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "tcp_client_node");

    ros::NodeHandle nh;

    // 设置信号处理
    signal(SIGINT, signalHandler);

    TcpClient client(nh,"192.168.7.225", "5000");

    if (!client.connectToServer()) {
        return 1;
    }

    client.startReceiving();

    // 主循环
    ros::Rate loop_rate(10); // 10 Hz
    while (ros::ok() && !client.get_shutdown_flag_()) {
        ros::spinOnce();
        loop_rate.sleep();
    }

    client.stop();
    client.joinReceiving();

    return 0;
}
